/*
---------------------------------------
By: 	Wei YE ( ye0003ei@e.ntu.edu.sg )

Date:	20/Aug/2012

-----------------------------------------
*/
#include "features.h"

//-------------------------------------
//   Functions for Class -- HsiHist
//-------------------------------------

void HsiHist::calcHist(const Mat& img, Mat& aHist, Mat& spADM, vector<int>& ptLabels, vector<int>& spSize)
{
	float* hsi = new float[3];
	int binNum = hbins_*sbins_*ibins_;
	int spNum  = spADM.rows;
	int i, j, bin, sp, sp1, sp2;

	aHist = Mat(spNum, 2*binNum, CV_32FC1, Scalar(0.0f));
	Mat sHist = aHist(Range::all(), Range(0, binNum));
	Mat nHist = aHist(Range::all(), Range(binNum, 2*binNum));

	// compute histogram for each superpixel
	for(i = 0; i < img.rows; i++){
		for(j = 0; j < img.cols; j++){	
			sp = ptLabels.at(i*img.cols + j);
			rgb2hsi(img.at<Vec3b>(i,j)[2], img.at<Vec3b>(i,j)[1], img.at<Vec3b>(i,j)[0], hsi); // count for each bin
			bin = cvFloor(hsi[0]*hbins_) + cvFloor(hsi[1]*sbins_)*hbins_ + cvFloor(hsi[2]*ibins_)*hbins_*sbins_;
			sHist.at<float>(sp, bin)++;
		}
	}

	for(sp1 = 0; sp1 < spNum; sp1++){
		int pixelNum = spSize.at(sp1);
		sHist.row(sp1).copyTo(nHist.row(sp1));
		for(sp2 = 0; sp2 < spNum; sp2++){
			if(spADM.at<int>(sp1,sp2) > 0){
				nHist.row(sp1) += sHist.row(sp2);
				pixelNum += spSize.at(sp2);
			}
		}
		nHist.row(sp1) /= pixelNum;
	}

	for(i = 0; i < spNum; i++) // normalize
		sHist.row(i) /= spSize.at(i);
	delete []hsi;
}

//-------------------------------------
//    Functions for Class -- HoG
//-------------------------------------

HoG::HoG() {
	bins = NUM_BINS;
	binsize = (PI * 80.0f)/float(bins);;

	g_w = 5;
	Gauss = cvCreateMat( g_w, g_w, CV_32FC1 );
	double a = -(g_w-1)/2.0;
	double sigma2 = 2*(0.5*g_w)*(0.5*g_w);
	double count = 0;
	for(int x = 0; x<g_w; ++x) {
		for(int y = 0; y<g_w; ++y) {
			double tmp = exp(-( (a+x)*(a+x)+(a+y)*(a+y) )/sigma2);
			count += tmp;
			cvSet2D( Gauss, x, y, cvScalar(tmp) );
		}
	}
	cvConvertScale( Gauss, Gauss, 1.0/count);

	ptGauss = new float[g_w*g_w];
	int i = 0;
	for(int y = 0; y<g_w; ++y) 
		for(int x = 0; x<g_w; ++x)
			ptGauss[i++] = (float)cvmGet( Gauss, x, y );

}


void HoG::extractOBin(IplImage* Iorient, IplImage* Imagn, Mat& out, vector<int>& ptLabels) 
{
	double* desc = new double[bins];
	// get pointers to orientation, magnitude
	int step;
	uchar* ptOrient;
	uchar* ptOrient_row;
	cvGetRawData( Iorient, (uchar**)&(ptOrient), &step);
	step /= sizeof(ptOrient[0]);

	uchar* ptMagn;
	uchar* ptMagn_row;
	cvGetRawData( Imagn, (uchar**)&(ptMagn));

	int off_w = int(g_w/2.0); 

	for(int y = 0;y < Iorient->height-g_w; y++, ptMagn+=step, ptOrient+=step) 
	{
		// Get row pointers
		ptOrient_row = &ptOrient[0];
		ptMagn_row = &ptMagn[0];

		for(int x = 0; x < Iorient->width-g_w; ++x, ++ptOrient_row, ++ptMagn_row) {
			calcHoGBin( ptOrient_row, ptMagn_row, step, desc );
			for(int l = 0; l < bins; ++l) {
				out.at<float>(ptLabels.at(y*Iorient->width + x), l) += (float)desc[l];
			}
		}
	}
	
	for(int i = 0; i < out.rows; i++){
		double s = 0;
		for(int j = 0; j < out.cols; j++)
			s += out.at<float>(i,j);
		for(int k = 0; k < out.cols; k++)
			out.at<float>(i,k) /= (float)s;
	}
	delete[] desc;
}

//-------------------------------------
//    Functions for Class -- Features
//-------------------------------------

void Features::Record(string file)
{
	ofstream feat;
	feat.open(file.c_str(), ios::out);
	for(int i = 0; i < vecChannels_.size(); i++){
		for(int j = 0; j < (*vecChannels_.at(i)).rows; j++){
			for(int k = 0; k < (*vecChannels_.at(i)).cols; k++){
				feat<<setiosflags(ios::fixed)<<(*vecChannels_.at(i)).at<float>(j,k)<<"+";
			}
			feat<<endl;
		}
		feat<<"------------------------------------------------------------"<<endl;
	}
	feat.close();
}
//----------------------------------------//
//    Set up spCent_, spADM_, spSize_     //
//----------------------------------------//
void Features::Setup(int height, int width)
{
	if(height*width != (int)ptLabels_.size())
		return;

	int i,j,k, label, alabel, rlabel; 
	spNum_ = ptLabels_.at(argmax(ptLabels_)) + 1;
	spADM_ = Mat(spNum_, spNum_, CV_32SC1, Scalar::all(0));
	spSize_.clear();
	spCent_.clear();
	spSize_.resize(spNum_, 0);
	spCent_.resize(spNum_, Point());

	for(i = 0; i < height; i++){
		for(j = 0; j < width; j++){
			label  = ptLabels_.at(i*width + j);
			spSize_.at(label)++;
			spCent_.at(label) += Point(j, i);

			if(i > 0 && j > 0 && i < height-1 && j < width -1){
				alabel = ptLabels_.at((i-1)*width + j);
				rlabel = ptLabels_.at(i*width + j + 1);
				if(label != alabel){
					spADM_.at<int>(label, alabel)++;
					spADM_.at<int>(alabel, label)++;
				}
				if(label != rlabel){
					spADM_.at<int>(label, rlabel)++;
					spADM_.at<int>(rlabel, label)++;
				}
			}
		}
	}

	for(k = 0; k < spNum_; k++)
		spCent_.at(k) *= (1.0f/spSize_.at(k));

}

void Features::ExtractFromImage(Mat& image)
{
	SLICSeg(image, ptLabels_);
	Setup(image.rows, image.cols);

	// Channel 0, H histogram
	HsiHist hist;
	hist.setBinNum(8,1,1);
	hist.calcHist(image, *(vecChannels_.at(0)), spADM_, ptLabels_, spSize_);

	// Channel 1, S histogram
	hist.setBinNum(1,8,1);
	hist.calcHist(image, *(vecChannels_.at(1)), spADM_, ptLabels_, spSize_);

	// Channel 2, I histgoram
	hist.setBinNum(1,1,8);
	hist.calcHist(image, *(vecChannels_.at(2)), spADM_, ptLabels_, spSize_);

	// Channel 3, HoG histogram
	HoG hog;
	*(vecChannels_.at(3)) = Mat(spNum_, hog.binNum(), CV_32SC1, Scalar::all(0));
	Mat gray_image;
	cvtColor(image, gray_image, CV_RGB2GRAY );
	medianBlur( gray_image, gray_image, CV_MEDIAN);
	
	Mat I_x = Mat(image.size(), CV_16SC1);  // Temporary images for computing I_x, I_y 
	Mat I_y = Mat(image.size(), CV_16SC1);
	Mat magnit = Mat(image.size(), CV_8UC1);
	Mat orient = Mat(image.size(), CV_8UC1);

	Sobel(gray_image, I_x, CV_16S, 1, 0, 3);   
	Sobel(gray_image, I_y, CV_16S, 0, 1, 3);

	for(int y = 0; y < image.rows; y++){    // Orientation of gradients
		for(int x = 0; x < image.cols; x++ ) {
			float tx = (float)I_x.at<short>(y, x) + (0.000001f * (I_x.at<short>(y, x) > 0 ? 1:-1));
			float ty = (float)I_y.at<short>(y, x);
			orient.at<uchar>(y, x) = uchar( ( atan((float)ty/tx)+3.14159265f/2.0f ) * 80 ); // Scaling [-pi/2 pi/2] -> [0 80*pi]
		}
	}
	
	for(int y = 0; y < image.rows; y++){   // Magnitude of gradients
		for(int x = 0; x < image.cols; x++ ) {
			float tx = (float)I_x.at<short>(y, x);
			float ty = (float)I_y.at<short>(y, x);
			magnit.at<uchar>(y,x) = (uchar)(sqrt(tx*tx + ty*ty)); 
		}
	}

	// 9-bin HOG feature stored vecChannels_[3]
	hog.extractOBin(&IplImage(orient), &IplImage(magnit), *(vecChannels_.at(3)), ptLabels_);
}
